#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Hubs,  S2, HTMotor,  HTServo,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Sensor, S3,     seekerL,        sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S4,     seekerR,        sensorHiTechnicIRSeeker1200)
#pragma config(Motor,  mtr_S1_C1_1,     fl,            tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     fr,            tmotorTetrix, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     bl,            tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     br,            tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S2_C1_1,     aux1,          tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C1_2,     aux2,          tmotorTetrix, PIDControl, reversed, encoder)
#pragma config(Servo,  srvo_S2_C2_1,    grab1,                tServoStandard)
#pragma config(Servo,  srvo_S2_C2_2,    grab2,                tServoStandard)
#pragma config(Servo,  srvo_S2_C2_3,    rampl,                tServoContinuousRotation)
#pragma config(Servo,  srvo_S2_C2_4,    rampr,                tServoContinuousRotation)
#pragma config(Servo,  srvo_S2_C2_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S2_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"

void holder()
{
	motor[br] = 0;
	motor[bl] = 0;
	motor[fr] = 0;
	motor[fl] = 0;
	wait1Msec(500);
}



void rightSpeed(int power)
{
	motor[fr]=power;
	motor[br]=power;
	return;
}
void leftSpeed(int power)
{
	motor[fl]=power;
	motor[bl]=power;
	return;
}

void initalizeEncoders()
{
	nMotorEncoder[bl] = 0;
	nMotorEncoder[br] = 0;
	nMotorEncoder[fl] = 0;
	nMotorEncoder[fr] = 0;
	wait1Msec(50);

	nMotorEncoder[bl] = 0;
	nMotorEncoder[br] = 0;
	nMotorEncoder[fl] = 0;
	nMotorEncoder[fr] = 0;
	wait1Msec(50);

	nMotorEncoder[bl] = 0;
	nMotorEncoder[br] = 0;
	nMotorEncoder[fl] = 0;
	nMotorEncoder[fr] = 0;
	wait1Msec(50);

	return;
}

task main()
{
	waitForStart();


	initalizeEncoders();

	nMotorEncoderTarget[bl] = 1050;       //forward
	nMotorEncoderTarget[br] = -1050;
	nMotorEncoderTarget[fl] = 1050;
	nMotorEncoderTarget[fr] = -1050;


	leftSpeed(50);
	rightSpeed(50);

	while((nMotorRunState[bl] != runStateIdle) || (nMotorRunState[br] != runStateIdle) || (nMotorRunState[fl] != runStateIdle) || (nMotorRunState[fr] != runStateIdle))
	{

	}

	holder();



	initalizeEncoders();

	nMotorEncoderTarget[bl] = 900;       //turn
	nMotorEncoderTarget[br] = 900;
	nMotorEncoderTarget[fl] = 900;
	nMotorEncoderTarget[fr] = 900;


	leftSpeed(50);
	rightSpeed(-50);

		while((nMotorRunState[bl] != runStateIdle) || (nMotorRunState[br] != runStateIdle) || (nMotorRunState[fl] != runStateIdle) || (nMotorRunState[fr] != runStateIdle))
	{

	}


	holder();

	initalizeEncoders();

	nMotorEncoderTarget[bl] = 3650;       //forward
	nMotorEncoderTarget[br] = -3650;
	nMotorEncoderTarget[fl] = 3650;
	nMotorEncoderTarget[fr] = -3650;




	leftSpeed(50);
	rightSpeed(50);

	while((nMotorRunState[bl] != runStateIdle) || (nMotorRunState[br] != runStateIdle) || (nMotorRunState[fl] != runStateIdle) || (nMotorRunState[fr] != runStateIdle))
	{

	}


	holder();



	initalizeEncoders();

  nMotorEncoderTarget[bl] = -1650;       //turn
	nMotorEncoderTarget[br] = -1650;
	nMotorEncoderTarget[fl] = -1650;
	nMotorEncoderTarget[fr] = -1650;



	leftSpeed(-50);
	rightSpeed(50);

	while((nMotorRunState[bl] != runStateIdle) || (nMotorRunState[br] != runStateIdle) || (nMotorRunState[fl] != runStateIdle) || (nMotorRunState[fr] != runStateIdle))
	{

	}


	holder();


	initalizeEncoders();


  leftSpeed(0);
	rightSpeed(0);
	motor[aux1] = 50;
	motor[aux2] = 50;
	wait1Msec(1900);

	motor[aux1] = 0;
	motor[aux2] = 0;
	wait1Msec(300);


	holder();

	initalizeEncoders();

	nMotorEncoderTarget[bl] = 4900;
	nMotorEncoderTarget[br] = -4900;
	nMotorEncoderTarget[fl] = 4900;
	nMotorEncoderTarget[fr] = -4900;



	leftSpeed(50);
	rightSpeed(50);

	while((nMotorRunState[bl] != runStateIdle) || (nMotorRunState[br] != runStateIdle) || (nMotorRunState[fl] != runStateIdle) || (nMotorRunState[fr] != runStateIdle))
	{

	}

	holder();

	motor[aux1] = 50;
	motor[aux2] = 50;
	wait1Msec(300);

	motor[aux1] = 0;
	motor[aux2] = 0;
	wait1Msec(300);

	holder();

	nMotorEncoderTarget[bl] = -1440;
	nMotorEncoderTarget[br] = 1440;
	nMotorEncoderTarget[fl] = -1440;
	nMotorEncoderTarget[fr] = 1440;


	leftSpeed(-50);
	rightSpeed(-50);

	while((nMotorRunState[bl] != runStateIdle) || (nMotorRunState[br] != runStateIdle) || (nMotorRunState[fl] != runStateIdle) || (nMotorRunState[fr] != runStateIdle))
	{

	}



	holder();

}
